#include "L298Driver.h"

L298Driver::L298Driver(
uint8_t pinA,uint8_t pinB,
uint8_t pinC,uint8_t pinD,
uint8_t pinEN1,uint8_t pinEN2):
_pinA(pinA),_pinB(pinB),_pinC(pinC),_pinD(pinD),
_pinEN1(pinEN1),_pinEN2(pinEN2)
{
  pinMode(_pinA,OUTPUT);
  pinMode(_pinB,OUTPUT);
  pinMode(_pinC,OUTPUT);
  pinMode(_pinD,OUTPUT);
  pinMode(_pinEN1,OUTPUT);
  pinMode(_pinEN2,OUTPUT);
  pinMode(_pinSENS1,INPUT);
  pinMode(_pinSENS2,INPUT);
  
  digitalWrite(_pinEN1,LOW);
  digitalWrite(_pinEN2,LOW);
  digitalWrite(_pinA,LOW);
  digitalWrite(_pinB,LOW);
  digitalWrite(_pinC,LOW);
  digitalWrite(_pinD,LOW);
}


L298Driver::L298Driver(
uint8_t pinA,uint8_t pinB,
uint8_t pinC,uint8_t pinD,
uint8_t pinEN1,uint8_t pinEN2,
uint8_t pinSENS1, uint8_t pinSENS2):
_pinA(pinA),_pinB(pinB),_pinC(pinC),_pinD(pinD),
_pinEN1(pinEN1),_pinEN2(pinEN2),
_pinSENS1(pinSENS1),_pinSENS2(pinSENS2){
  pinMode(_pinA,OUTPUT);
  pinMode(_pinB,OUTPUT);
  pinMode(_pinC,OUTPUT);
  pinMode(_pinD,OUTPUT);
  pinMode(_pinEN1,OUTPUT);
  pinMode(_pinEN2,OUTPUT);
  pinMode(_pinSENS1,INPUT);
  pinMode(_pinSENS2,INPUT);
  
  digitalWrite(_pinEN1,LOW);
  digitalWrite(_pinEN2,LOW);
  digitalWrite(_pinA,LOW);
  digitalWrite(_pinB,LOW);
  digitalWrite(_pinC,LOW);
  digitalWrite(_pinD,LOW);  
  
  pinMode(_pinSENS1,INPUT);
  pinMode(_pinSENS2,INPUT);
}

void L298Driver::Off(uint8_t channel) {
  if(channel&L298_A_CHANNEL) {
    digitalWrite(_pinEN1,LOW);
  }
  if(channel&L298_B_CHANNEL) {
    digitalWrite(_pinEN2,LOW);
  }
}

void L298Driver::On(uint8_t channel) {
  if(channel&L298_A_CHANNEL) {
    digitalWrite(_pinEN1,HIGH);
  }
  if(channel&L298_B_CHANNEL) {
    digitalWrite(_pinEN2,HIGH);
  }
}
void L298Driver::Run(uint8_t channel, uint8_t speed) {
  if(channel&L298_A_CHANNEL) {
    analogWrite(_pinEN1,speed);
  }
  if(channel&L298_B_CHANNEL) {
    analogWrite(_pinEN2,speed);
  }
}

void L298Driver::Stop(uint8_t channel) {
  if(channel&L298_A_CHANNEL) {
    analogWrite(_pinEN1, 0);
    digitalWrite(_pinEN1,LOW);
  }
  if(channel&L298_B_CHANNEL) {
    analogWrite(_pinEN2, 0);
    digitalWrite(_pinEN2,LOW);
  }
}

void L298Driver::Set(uint8_t channel, uint8_t state) {
  switch(channel) {
  case L298_A_CHANNEL:
      digitalWrite(_pinA,state & 0x01);
      digitalWrite(_pinB,state & 0x02); 
      break;
  case L298_B_CHANNEL:
      digitalWrite(_pinC,state & 0x01);
      digitalWrite(_pinD,state & 0x02); 
      break;
  }
}

uint16_t L298Driver::ReadVSens(uint8_t channel) {
  switch(channel) {
  case L298_A_CHANNEL:
      return analogRead(_pinSENS1); 
  case L298_B_CHANNEL:
      return analogRead(_pinSENS2); 
  }
}
